﻿#include "pathexcutequeue.h"
#include <QMutexLocker>
#include <QTimer>
#include <QEventLoop>

#include "componentmanager.h"
#include "component_log.h"
#include "../../pipeline/pipeline.h"

PathExcuteQueue::PathExcuteQueue(QObject* parent)
	: QObject(parent)
{}

PathExcuteQueue::~PathExcuteQueue()
{}

PathBuffInfo PathExcuteQueue::GetPathBuffInfo()
{
	PathBuffInfo info = {};
	if (m_exec.valied) {
		info.execpath = m_exec.pathName;
		info.execpipe = m_exec.obj->GetPipelineId();
	}

	if (m_wait.valied) {
		info.waitpath = m_wait.pathName;
		info.waitpipe = m_wait.obj->GetPipelineId();
	}
	return info;
}

ExecPathList PathExcuteQueue::GetExecPathList()
{
	ExecPathList info = {};
	if (m_exec.valied) {
		for (auto& iter : m_exec.movePath) {
			info.paths.insert(iter.id, iter.name);
			info.currpoint = iter.id;
		}
		if (m_exec.path.size() > 0) {
			info.currpoint = m_exec.path.at(0).id;
		}
	}

	return info;
}


bool PathExcuteQueue::Init()
{
	m_rob = dynamic_cast<Robot*>(component->GetComponent(Component::component_robot));
	if (m_rob == nullptr) {
		log_error("Get Robot component failed");
		return false;
	}
	bool ret = m_rob->RegisterSubscriberFunction((int)RobotMsgType::robot_msg_point_id,
		[&](const QByteArray& data) {
			return this->PointIdCallback(data);
		});
	if (!ret) {
		log_error("Register Callback to robot failed");
		return false;
	}

	m_execThread = new std::thread(&PathExcuteQueue::PathExcuting, this);

	if (m_execThread->joinable()) {
		m_execThread->detach();
		log_trace(u8"任务调度启动");
	} else {
		log_error(u8"任务调度线程无法启动", false);
		return false;
	}

	return true;
}

bool PathExcuteQueue::PathStop()
{
	log_error("PathExcuteQueue Stop");
	m_stopFlag = true;
	return true;
}

bool PathExcuteQueue::PathStart()
{
	log_error("PathExcuteQueue start");
	m_stopFlag = false;
	return true;
}

/****************************************************************************
 * 功能: 路径继续执行
 * 传参:
 * 返回值: 成功失败
 ****************************************************************************/
bool PathExcuteQueue::PathContinueExcute()
{
	// 继续执行，就是将m_exec里面的未完成路径重新下发一遍，其余参数保持不变
	log_error("PathExcuteQueue continue");
	return true;
}

/****************************************************************************
 * 功能: 路径重新开始
 * 传参:
 * 返回值: 成功失败
 ****************************************************************************/
bool PathExcuteQueue::PathReStartExcute()
{
	// 清除当前队列中所有的路径信息等待重新下发新数据
	//ClearCacheBuff();
	ClearExecBuff();
	ClearWaitBuff();
	return true;
}


/****************************************************************************
 * 功能: 判断路径执行队列是否可以添加路径
 * 传参:
 * 返回值: true:表可以继续添加路径，false:不可以
 ****************************************************************************/
bool PathExcuteQueue::IfPathQueueIsIdle()
{
	if (m_wait.valied) {	// cachebuff中数据有效，表面队列已满
		return false;
	}

	return true;
}

/****************************************************************************
 * 功能: 添加路径到队列
 * 传参: pipe: 路径的归属业务线指针，path: 路径数据
 * 返回值: 0：添加成功， -1：参数异常，-99表示添加失败（队列已满）
 ****************************************************************************/
int PathExcuteQueue::AddPathToQueue(Pipeline* pipe, const QString& path)
{
	if (m_wait.valied) {
		log_error("Path Cache buff is full, not allow to add pth");
		return -99;
	}
	if (pipe == nullptr) {
		log_error("add path failed, because of pipe is nullptr");
		return -1;
	}

	PathJ pathInfo = {};
	pathInfo.fromJson(path.toUtf8());
	if (pathInfo.name().isEmpty() || pathInfo.path().size() <= 0) {
		log_error("add path failed, because of path data is error; path = {}", path);
		return -1;
	}

	QMutexLocker locker(&m_buffLock); // 加锁防止出现add操作和get操作同时执行
	if (AddPathToWaitBuff(pathInfo, pipe)) {
		return 0;
	}

	return -99;
}

/*等待路径执行完成*/
#define MAXTIME (10 * 60 * 2) // 10 分钟路径未执行完成则失败
bool PathExcuteQueue::WaitForPathFinished()
{
	// 500 ms 检查一次
	int times = MAXTIME;
	while (times > 0) {
		QList<int> temp; // 读取已经走完的路径点
		{
			QMutexLocker lock(&pointIdQueueLock);
			temp = m_pointIdQueue;
			m_pointIdQueue.clear();
		}
		while (!temp.isEmpty()) {
			int id = temp.takeFirst();
			for (int i = 0; i < m_exec.path.size(); i++) {
				/*在路径列表中寻找对应id,清除路径数据*/
				if (m_exec.path[i].id == id) {
					int j = 0;
					while (j <= i) {
						auto point = m_exec.path.takeFirst();
						log_trace("path {} point {} id[{}] is over", m_exec.pathName, point.name, point.id);
						if (!PathActionExcute(point.id)) {
							SetPathExcuteEvent(QString("路径%1的路径点%2路径动作执行失败")
								.arg(m_exec.pathName).arg(point.id));
							return false;
						}
						j++;
					}
					break;
				}
			}
			times = MAXTIME;	// 重置等待时间
		}
		// 执行路径为空时，路径执行结束
		if (m_exec.path.isEmpty()) {
			log_trace("{} path excute finished", m_exec.pathName);
			return true;
		}
		if (!m_rob->GetStopMoveState()) {
			/*判断机械手当前是不是停止运动的状态*/
			SetPathExcuteEvent("机械手停止，路径无法执行");
			return false;
		}

		/*执行500ms等待*/
		QEventLoop loop;
		QTimer::singleShot(500, &loop, &QEventLoop::quit);
		loop.exec();
	}
	log_error("wait for path excute overtime");
	SetPathExcuteEvent("路径执行超时");
	return false;
}

/*路径动作执行函数*/
bool PathExcuteQueue::PathActionExcute(int id)
{
	if (m_exec.action.contains(id)) {
		auto act = m_exec.action[id];
		switch (act.type) {
		case 1:	// 夹具动作
		{
			GripParam param = {};
			param.index = act.param.toInt();
			if (!m_rob->SetGrip(param.toByteArray())) {
				log_error("路径{}路径点{}，夹具动作{}执行失败", m_exec.pathName, id, param.index());
				return false;
			}
			return true;
		}
		break;
		case 2:	// 拍照动作和发送通知
		case 3:	// 由pipeline执行	
			m_exec.obj->PathAction(act.type, act.param);	// 不考虑业务线动作执行是否成，由pipeline自己控制
			return true;
			break;
		default:
			break;
		}

	}
	return true;
}
int PathExcuteQueue::PointIdCallback(const QByteArray& data)
{
	int id = data.toInt();
	if (id <= 0 || !m_exec.valied) {
		return 0;
	}
	QMutexLocker lock(&pointIdQueueLock);
	m_pointIdQueue.append(id);
	return 0;
}


/*执行buff对应函数*/
void PathExcuteQueue::PathExcuting()	// 路径执行线程
{
	log_trace("*****************PathExcuting start***************")
		while (1) {
			std::unique_lock<std::mutex> lock(excuteLock);
			if (!m_exec.valied) {	// exec buff为空线程等待
				execCond.wait(lock);
			}
			if (m_stopFlag || !m_exec.valied) {
				continue;
			}

			/*路径开始处理*/
			if (!m_exec.obj->PathStart(m_exec.pathName)) {
				// 路径启动条件不足，等待500ms后，重新下发
				QEventLoop loop;
				QTimer::singleShot(500, &loop, &QEventLoop::quit);
				loop.exec();
				continue;
			}
			{	// 路径下发前，清理路径ID队列
				QMutexLocker queloc(&pointIdQueueLock);
				m_pointIdQueue.clear();
			}
			/*下发路径*/
			bool ret = m_rob->MoveJL(m_exec.path);
			if (!ret) {	// 路径下发失败
				log_error("path send failed");
				m_stopFlag = true;
				SetPathExcuteEvent("路径下发失败");
				continue;
			}
			/*等待路径执行结束*/
			if (!WaitForPathFinished()) {
				log_error("wait path finish error");
				m_stopFlag = true;
				continue;
			}

			/*路径结束处理*/
			if (!m_exec.obj->PathFinished(m_exec.pathName)) {
				// 路径结束处理失败
				log_error("path {} path finished failed", m_exec.pathName);
				SetPathExcuteEvent("路径完成处理操作执行出错");
				m_stopFlag = true;
				continue;
			}
			/*路径正常执行完成*/
			ClearExecBuff();
			// 获取下一条可以执行路径
			ExecBuffGetPath();
		}
	log_trace("*****************PathExcuting stop***************");
}
bool PathExcuteQueue::AddPathToExecBuff()	// 添加路径到执行buff
{
	if (m_exec.valied) {
		return false;
	}
	m_exec = m_wait;
	m_exec.valied = true;
	log_trace("{} add to exec buff", m_exec.pathName);
	// 添加成功后，发送线程启动通知
	execCond.notify_all();
	return true;
}
bool PathExcuteQueue::ExecBuffGetPath()		// 执行buff获取路径
{
	if (m_exec.valied) {
		return false;
	}

	return GetPathFromWaitBuffToExec();
}

void PathExcuteQueue::SetPathExcuteEvent(const QString& err)
{
	m_stopFlag = true;
	QString str = QString("%1的路径%2执行出错%3").
		arg(m_exec.obj->GetPipelineId()).arg(m_exec.pathName).arg(err);
	log_error("{}", str);
	emit PathExcutedFailed(str);
}

void PathExcuteQueue::SetPathParseEvent()
{
	m_stopFlag = true;
	QString str = QString("%1的路径%2解析出错, 无法执行当前路径").
		arg(m_wait.obj->GetPipelineId()).arg(m_wait.pathName);
	log_error("{}", str);
	emit PathExcutedFailed(str);
}

/*WaitBuff 对应函数*/
/*校验moveJlinfo是否合理*/
bool PathExcuteQueue::CheckMoveJLInfo(const MoveJLInfo& info)
{
	log_trace("move info =[id:{}, type:{}, acc:{}, vel:{}, zone:{}, di:{}]",
		info.id, info.pointType, info.acc, info.vel, info.zone, info.di);
	log_trace("move pos =[pos:({}, {}, {}, {}, {}, {})]", info.pos.x, info.pos.y, info.pos.z,
		info.pos.a, info.pos.b, info.pos.c)
		if (info.id <= 0) {
			log_error("id error");
			return false;
		}
	if (info.pointType < 1 || info.pointType > 2) {
		log_error("pointType error");
		return false;
	}

	if (info.acc <= 0 || info.vel <= 0) {
		log_error("acc || vel error");
		return false;
	}
	if (info.zone < 0 || info.di != -1) {
		log_error("zone || di error");
		return  false;
	}

	if (info.pos.x < -5000.0f || info.pos.x > 5000.0f) {
		log_error("pos.x error");
		return false;
	}
	if (info.pos.y < -5000.0f || info.pos.y > 5000.0f) {
		log_error("pos.y error");
		return false;
	}
	if (info.pos.z < -5000.0f || info.pos.z > 5000.0f) {
		log_error("pos.z error");
		return false;
	}
	if (info.pos.a < -360.0f || info.pos.a > 360.0f) {
		log_error("pos.a error");
		return false;
	}

	//if (std::abs(info.pos.b - 0.0f) > 0.001f) {
	//	log_error("pos.b error");
	//	return false;
	//}

	//if (std::abs(info.pos.c - 180.0f) > 0.001f) {
	//	log_error("pos.c error");
	//	return false;
	//}

	return true;
}


bool PathExcuteQueue::AddPathToWaitBuff(const PathJ& path, Pipeline* pipe)
{
	if (m_wait.valied) {
		log_error("wait buff is full");
		return false;
	}
	m_wait.obj = pipe;
	m_wait.pathName = path.name();
	for (auto& iter : path.path()) {
		MoveJLInfo move = {};
		move.TransFromMoveJLInfoJ(iter.move());
		if (!CheckMoveJLInfo(move)) {
			SetPathParseEvent();
			ClearWaitBuff();
			return false;
		}
		m_wait.path.append(move);
		m_wait.movePath.insert(iter.id(), move);
		ActionInfo act = {};
		act.type = iter.action().type();
		act.param = iter.action().param();
		if (act.type > 0 && act.type <= 3) { // 动作类型取值范围1：夹具动作，2：拍照 3：发送通知
			m_wait.action.insert(iter.id(), act);
		}	// 无动作不需要添加
	}
	m_wait.valied = true;
	log_trace("{} add to wait buff", m_wait.pathName);
	if (AddPathToExecBuff()) {
		ClearWaitBuff();
	}

	return true;
}
bool PathExcuteQueue::GetPathFromWaitBuffToExec()
{
	if (!m_wait.valied) {
		return false;
	}

	m_exec = m_wait;
	m_exec.valied = true;
	log_trace("{} add to exec buff", m_exec.pathName);
	ClearWaitBuff();
	/*此处无需发送线程通知，因为该函数由线程直接调用*/
	return true;
}


///*CacheBuff对应函数*/
//bool PathExcuteQueue::AddPathToCacheBuff(const PathJ& path, Pipeline* pipe)
//{
//	if (m_cache.valied) {
//		log_error("Path Cache buff is full, not allow to add pth");
//		return false;
//	}
//	m_cache.valied = true;
//	m_cache.obj = pipe;
//	m_cache.pathName = path.name();
//	m_cache.path = path.path();
//	log_trace("{} add to cache buff", m_cache.pathName);
//	if (AddPathToWaitBuff()) {	// 路径添加到等待buff成功后，清除缓存buf中的数据
//		ClearCacheBuff();
//	}
//	return true;
//}
//
//bool PathExcuteQueue::GetPathFormCacheBuff()
//{
//	if (!m_cache.valied) {
//		return false;
//	}
//	m_wait.obj = m_cache.obj;
//	m_wait.pathName = m_cache.pathName;
//	for (auto& iter : m_cache.path) {
//		MoveJLInfo move = {};
//		move.TransFromMoveJLInfoJ(iter.move());
//		if (!CheckMoveJLInfo(move)) {
//			SetPathParseEvent();
//			ClearWaitBuff();
//			return false;
//		}
//		m_wait.path.append(move);
//		m_wait.movePath.insert(iter.id(), move);
//		ActionInfo act = {};
//		act.type = iter.action().type();
//		act.param = iter.action().param();
//		if (act.type > 0 && act.type <= 3) { // 动作类型取值范围1：夹具动作，2：拍照 3：发送通知
//			m_wait.action.insert(iter.id(), act);
//		}	// 无动作不需要添加
//	}
//	m_wait.valied = true;
//	log_trace("{} add to wait buff", m_wait.pathName);
//
//	return true;
//}


//void PathExcuteQueue::ClearCacheBuff()
//{
//	m_cache.valied = false;
//	m_cache.pathName.clear();
//	m_cache.obj = nullptr;
//	m_cache.path.clear();
//}
void PathExcuteQueue::ClearWaitBuff()
{
	m_wait.valied = false;
	m_wait.pathName.clear();
	m_wait.obj = nullptr;
	m_wait.path.clear();
	m_wait.movePath.clear();
	m_wait.action.clear();
}
void PathExcuteQueue::ClearExecBuff()
{
	m_exec.valied = false;
	m_exec.pathName.clear();
	m_exec.obj = nullptr;
	m_exec.path.clear();
	m_exec.movePath.clear();
	m_exec.action.clear();
}
